# Note: Line detection is done by using the Hough Transform:
# http://en.wikipedia.org/wiki/Hough_transform
# Please read about it above for more information on what `theta` and `rho` are.

# find_lines() finds infinite length lines. Use find_line_segments() to find non-infinite lines.

import sensor
import time
import math
from pyb import UART
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time=2000)
clock = time.clock()
uart=UART(3,9600)
mindegree=0
maxdegree=179
tsd=(0,20,-128,61,-128,127)
maintimes=0
maintimescountdown=0
# All line objects have a `theta()` method to get their rotation angle in degrees.
# You can filter lines based on their rotation angle.
# All lines also have `x1()`, `y1()`, `x2()`, and `y2()` methods to get their end-points
# and a `line()` method to get all the above as one 4 value tuple for `draw_line()`.

def distance(l):
    yyy=0
    for li in l:
        if li.theta()>45 and li.theta()<135:
            yc=(li.y1()+li.y2())/2
            dis=(120-yc)*math.sin(math.pi*li.theta()/180)#########
            thth=li.theta()
            yyy=1
    if yyy:
        dis=math.floor(dis)
        return dis,thth
    else:return -1000,-1000

def count(img,times):
    countarea=(140-25*times,0,10,25)
    blackareas=img.find_blobs([tsd],roi=countarea,pixels_threshold=10, area_threshold=10, merge=True)
    img.draw_rectangle(countarea,[0,255,255])
    if blackareas:
        times+=1
    if times==4:
        uart.write("@S0#")
    return times

def send(nu):
    msg = "@D{}#\n".format(nu)
    uart.write(msg)
    print(msg)

# ====== 卡尔曼滤波相关 ======
class KalmanFilter:
    def __init__(self, process_noise, measurement_noise, estimate_error, estimate):
        self.q = process_noise  # 过程噪声，需调参
        self.r = measurement_noise  # 测量噪声，需调参
        self.p = estimate_error  # 估计误差，需调参
        self.x = estimate  # 初始估计值，需调参

    def update(self, measurement):
        # 预测更新
        self.p = self.p + self.q
        # 卡尔曼增益
        k = self.p / (self.p + self.r)
        # 更新估计值
        self.x = self.x + k * (measurement - self.x)
        # 更新估计误差
        self.p = (1 - k) * self.p
        return self.x
# ====== 卡尔曼滤波相关结束 ======

# 初始化卡尔曼滤波器，下面的参数都可以调节以适应你的实际情况
kalman = KalmanFilter(
    process_noise=0.24,      # 过程噪声Q，需调参
    measurement_noise=30, # 测量噪声R，需调参
    estimate_error=1,     # 初始估计误差P，需调参
    estimate=0           # 初始估计值x，需调参
)

while True:
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    if maintimescountdown>0:maintimescountdown-=1
    # `theta_margin` and `rho_margin` control merging similar lines. If two lines
    # theta and rho value differences are less than the margins then they are merged.
    lines=img.find_lines(theta_margin=2,rho_margin=10)
    for l in lines:
        #if (min_degree <= l.theta()) and (l.theta() <= max_degree):
        img.draw_line(l.line(), color=(255, 0, 0))
    #print("FPS %f" % clock.fps())
    if lines and maintimes<4:
        if maintimescountdown<1:
            maintimescountdown-=30*maintimes
            maintimes=count(img,maintimes)
            maintimescountdown+=30*maintimes
        dis,ththth=distance(lines)
        if dis!=-1000 and ththth!=-1000:
        # ====== 卡尔曼滤波处理距离 ======
            filtered_dis = int(kalman.update(dis))  # 这里得到滤波后的距离
            send(filtered_dis)  # 发送滤波后的距离
            print(maintimescountdown)
        # ====== 卡尔曼滤波处理结束 ======
# About negative rho values:A [theta:-rho] tuple is the same as [theta+180:+rho].
